/***************************************************
 @Name :        小铭同学
 @Time :        2022-10-30
 @Email :       LiaoMingWJ233@163.com
 @Comment :     灰度传感器功能的实现
 @FileName :    gray.c
 @Version :     1.0
****************************************************/


#include "gray.h"

/*------------------------ 通过灰度判断位置偏差 --------------------*/

/* 前进时候的位置偏差 */
signed char gray_front_error(void)
{
  u8 temp = (GRAY_F_L << 2) | (GRAY_F_M << 1) | GRAY_F_R;   //灰度拼接起来
  signed char err;    /* 位置偏差 */

  switch (temp)
  {
    case 0x01:          // 001 白线，左小
      err = -1;
      break;
    case 0x03:          // 011 白线，左大
      err = -2;
      break;
    case 0x04:          // 100 白线，右小
      err = 1;
      break;
    case 0x06:          // 110 白线，右大
      err = 2;
      break;
    default:            // 101 白线在中间，111 跑出去了
      err = 0;
      break;
  }
  return err;
}

signed char gray_front_error1(void)
{
  u8 temp = (GRAY_F_L << 2) | (GRAY_F_M << 1) | GRAY_F_R; //灰度拼接起来
  float/*signed char*/ err;    /* 位置偏差 */

  switch (temp)
  {
    case 0x01:          // 001 白线，左小
      err = -1;
      break;
    case 0x03:          // 011 白线，左大
      err = -1.1;
      break;
    case 0x04:          // 100 白线，右小
      err = 1;
      break;
    case 0x06:          // 110 白线，右大
      err = 1.1;
      break;
    default:            // 101 白线在中间，111 跑出去了
      err = 0;
      break;
  }
  return err;
}

/* 后退时候的位置偏差 */
signed char gray_back_error(void)
{
  signed char err;
  u8 temp = (GRAY_B_L << 2) | (GRAY_B_M << 1) | GRAY_B_R;

  switch (temp)
  {
    case 0x01:          //偏右，和上面一样
      err = -1;
      break;
    case 0x03:          //偏右+
      err = -2;
      break;
    case 0x04:          //偏左
      err = 1;
      break;
    case 0x06:          //偏左+
      err = 2;
      break;
    default:
      err = 0;
      break;
  }
  return err;
}

/* 判断左侧是否有线 */
bool gray_is_left(void)
{
  return
      ( GRAY_L_F && GRAY_L_M && GRAY_L_B ) == 0;
}

/* 判断右侧是否有线 */
bool gray_is_right(void)
{
  return
      ( GRAY_R_F && GRAY_R_M && GRAY_R_B ) == 0;
}

/* 判断是否是十字路口或十字标线，比较宽松的判断条件。使用&&可使条件变严格 */
bool gray_is_cross(void)
{
  return (gray_is_left() && gray_is_right()); 
			 /*(gray_is_left() || gray_is_right());*/
			/*((GRAY_L_F && GRAY_L_M && GRAY_L_B) ||
       (GRAY_R_F && GRAY_R_M && GRAY_R_B) ||
			 (GRAY_F_L && GRAY_F_M && GRAY_F_R && GRAY_B_L && GRAY_B_M && GRAY_B_R))== 0;*/
}

bool gray_is_cross1(void)
{
	return //(gray_is_left() && gray_is_right()); 
			 /*(gray_is_left() || gray_is_right());*/
			((GRAY_L_F && GRAY_L_M && GRAY_L_B) ||
       (GRAY_R_F && GRAY_R_M && GRAY_R_B) ||
			 (GRAY_F_L && GRAY_F_M && GRAY_F_R && GRAY_B_L && GRAY_B_M && GRAY_B_R))== 0;
}
bool gray_is_cross2(void)
{
	return //(gray_is_left() && gray_is_right()); 
			 (gray_is_left() || gray_is_right());
			/*((GRAY_L_F && GRAY_L_M && GRAY_L_B) ||
       (GRAY_R_F && GRAY_R_M && GRAY_R_B) ||
			 (GRAY_F_L && GRAY_F_M && GRAY_F_R && GRAY_B_L && GRAY_B_M && GRAY_B_R))== 0;*/
}

/* 判断是否为T字路口 */
bool gray_is_t(void)
{
    return /*(gray_is_left() || gray_is_right());*/
		(!(GRAY_F_L && GRAY_F_M && GRAY_F_R) ||
     ((GRAY_L_F && GRAY_L_M && GRAY_L_B) ||
      (GRAY_R_F && GRAY_R_M && GRAY_R_B))) == 0;	
		
//  return
//      ((GRAY_L_F && GRAY_L_M && GRAY_L_B) &&
//       (GRAY_R_F && GRAY_R_M && GRAY_R_B) ) == 0;  
//  return (!(GRAY_F_L && GRAY_F_M && GRAY_F_R) ||
//          ((GRAY_L_F && GRAY_L_M && GRAY_L_B) ||
//           (GRAY_R_F && GRAY_R_M && GRAY_R_B) ||
//           (GRAY_B_L && GRAY_B_M && GRAY_B_R))) == 0;
}

/* 判断转弯是否到位，可以直接用十字路口逻辑，这个条件更宽松 */
bool gray_turn_not_ok0(void)
{
  /* 车头对正白线，转弯就可以停止 */
  return (GRAY_F_M);
}

void gray_test(void)
{
  while (1)
  {
    if (gray_is_cross() == 1)
    {
      printf("cross\n");
    }
    if (gray_is_t() == 1)
    {
      printf("T\n");
    }
    if (gray_turn_not_ok0() == 0)
    {
      printf("ok0\n");
    }

    delay_ms(500);
    printf(".\n");
  }
}
